#import "Accelerometer.h"

#define FILTERINGFACTOR 0.1

@implementation Accel

-(void) SetupAccelerometer: (float)AccelerometerFrequency
{
	[[UIAccelerometer sharedAccelerometer] setUpdateInterval:(1.0 / AccelerometerFrequency)];
	[[UIAccelerometer sharedAccelerometer] setDelegate:self];
}

-(void) accelerometer:(UIAccelerometer*)accelerometer didAccelerate:(UIAcceleration*)Acceleration
{
	_accelerometer[0] = Acceleration.x * FILTERINGFACTOR + _accelerometer[0] * (1.0 - FILTERINGFACTOR);
	_accelerometer[1] = Acceleration.y * FILTERINGFACTOR + _accelerometer[1] * (1.0 - FILTERINGFACTOR);
	_accelerometer[2] = Acceleration.z * FILTERINGFACTOR + _accelerometer[2] * (1.0 - FILTERINGFACTOR);
}

-(void) GetAccelerometerMatrix:(GLfloat*)matrix
{
	GLfloat length = sqrtf(_accelerometer[0] * _accelerometer[0] + _accelerometer[1] * _accelerometer[1] + _accelerometer[2] * _accelerometer[2]);
	
	bzero(matrix, sizeof(matrix));
	matrix[15] = 1.0f;
	
	matrix[0] = _accelerometer[0] / length;
	matrix[1] = _accelerometer[1] / length;
	matrix[2] = _accelerometer[2] / length;
	
	matrix[4] = 0.0;
	matrix[5] = 1.0;
	matrix[6] = -_accelerometer[1] / _accelerometer[2];
	length = sqrtf(matrix[4] * matrix[4] + matrix[5] * matrix[5] + matrix[6] * matrix[6]);
	matrix[4] /= length;
	matrix[5] /= length;
	matrix[6] /= length;
	
	matrix[8] = matrix[1] * matrix[6] - matrix[2] * matrix[5];
	matrix[9] = matrix[4] * matrix[2] - matrix[6] * matrix[0];
	matrix[10] = matrix[0] * matrix[5] - matrix[1] * matrix[4];
}

-(void) GetAccelerometerVector:(double*)AccelValue;
{
	AccelValue[0] = (double)_accelerometer[0];
	AccelValue[1] = (double)_accelerometer[1];
	AccelValue[2] = (double)_accelerometer[2];
}

@end
